/*
Automatic clinic tests using a robot arm.
Copyright (C) 2010  CIC-CPC, CHU Gabriel Montpied
All other trademarks, logos and copyrights are the property
of their respective owners.

This file is part of cpc-painbot.

cpc-painbot is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

cpc-painbot is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with cpc-painbot.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "stimulation/vonfrey.h"

#include <QFinalState>
#include <QSignalTransition>

#include <QDebug>

#include "filament.h"
#include "bras.h"

class TransitionBool : public QSignalTransition
{
public:
    TransitionBool(Bras *bras) :
            QSignalTransition(bras, SIGNAL(estPret(bool))) {}

protected:
    bool eventTest(QEvent *event)
    {
        if (!QSignalTransition::eventTest(event))
            return false;

        QStateMachine::SignalEvent *se = static_cast<QStateMachine::SignalEvent*>(event);
        qDebug() << "TransitionBool pret :" << se->arguments().at(0).toBool();
        return se->arguments().at(0).toBool();
    }
};

inline void ajouterTransitionBool(Bras *bras, QState *source, QAbstractState *destination)
{
    TransitionBool *trans = new TransitionBool(bras);
    trans->setTargetState(destination);
    source->addTransition(trans);
}


VonFrey::VonFrey(QObject *parent) :
    Stimulateur(Stimulateur::VonFrey, parent)
{
    //setPosition(new PositionBras(0, 400));
}

void VonFrey::refaireTransitions()
{
    // Les différentes étapes de la stimulation.
    qDebug() << "VonFrey::refaireTransitions()";
    // Prend le filament
    QState *prendre = new QState(this);
        QState *sousFilament = new QState(prendre);
        QState *remonter = new QState(prendre);
        QState *degager = new QState(prendre);

    // Stimule le patient.
    QState *stimuler = new QState(this);
        QState *placer = new QState(stimuler);
        QState *toucher = new QState(stimuler);
        QState *attendre = new QState(stimuler);
        QState *relacher = new QState(stimuler);

    // Repose le filament sur le rack.
    QState *rendre = new QState(this);
        QState *aligner = new QState(rendre);
        QState *reposer = new QState(rendre);
        QState *descendre = new QState(rendre);
        QState *sortir = new QState(rendre);

    // Etat puits.
    QFinalState *final = new QFinalState(this);

    // Logique de transition.
    ajouterTransitionBool(bras_, sousFilament, remonter);
    ajouterTransitionBool(bras_, remonter, degager);
    ajouterTransitionBool(bras_, degager, stimuler);

    ajouterTransitionBool(bras_, placer, toucher);
    ajouterTransitionBool(bras_, toucher, attendre);
    attendre->addTransition(duree_, SIGNAL(timeout()), relacher);
    ajouterTransitionBool(bras_, relacher, rendre);

    ajouterTransitionBool(bras_, aligner, reposer);
    ajouterTransitionBool(bras_, reposer, descendre);
    ajouterTransitionBool(bras_, descendre, sortir);
    ajouterTransitionBool(bras_, sortir, final);

    // Etats initiaux.
    prendre->setInitialState(sousFilament);
    stimuler->setInitialState(placer);
    rendre->setInitialState(aligner);
    setInitialState(prendre);

    // Logique d'action.
    connect(sousFilament, SIGNAL(entered()), this, SLOT(placerSousFilament()));
    connect(remonter, SIGNAL(entered()), this, SLOT(attraperFilament()));
    connect(degager, SIGNAL(entered()), this, SLOT(degagerFilament()));

    connect(placer, SIGNAL(entered()), this, SLOT(placerFilament()));
    connect(toucher, SIGNAL(entered()), this, SLOT(poserFilament()));
    connect(attendre, SIGNAL(entered()), duree_, SLOT(start()));
    connect(relacher, SIGNAL(entered()), this, SLOT(releverFilament()));

    connect(aligner, SIGNAL(entered()), this, SLOT(placerDevantFilament()));
    connect(reposer, SIGNAL(entered()), this, SLOT(remettreFilament()));
    connect(descendre, SIGNAL(entered()), this, SLOT(placerSousFilament()));
    connect(sortir, SIGNAL(entered()), this, SLOT(degagerFilament()));

    // Arrêt en cas d'erreur
    connect(bras(), SIGNAL(erreur(int)), this, SLOT(stop()));
}

void VonFrey::placerDevantFilament()
{
    Q_ASSERT(bras_ != 0);
    Q_ASSERT(filament_ != 0);

    qDebug() << "placerDevantFilament";

    PositionBras pos = filament()->position();

    pos.deltaY(100);
    //pos.setZ(30);
    pos.setR(180);
    bras_->deplacer(pos, 50);
}

void VonFrey::placerSousFilament()
{
    Q_ASSERT(bras_ != 0);
    Q_ASSERT(filament_ != 0);

    qDebug() << "VonFrey : Se place sous le Filament.";
    PositionBras pos = filament()->position();

    pos.deltaZ(50);
    pos.setR(180);
    bras_->deplacer(pos, 50);
}

void VonFrey::attraperFilament()
{
    Q_ASSERT(bras_ != 0);
    Q_ASSERT(filament_ != 0);

    qDebug() << "VonFrey : Attrape le Filament";
    PositionBras pos = bras_->position();

    pos.setZ(filament()->position().z());
    bras_->deplacer(pos, 10);
}

void VonFrey::remettreFilament()
{
    Q_ASSERT(bras_ != 0);
    Q_ASSERT(filament_ != 0);

    qDebug() << "VonFrey : remet le Filament";
    PositionBras pos = filament()->position();

    pos.setR(180);

    bras_->deplacer(pos, 10);
}

void VonFrey::degagerFilament()
{
    Q_ASSERT(bras_ != 0);

    qDebug() << "VonFrey : dégage le Filament.";

    PositionBras pos = bras_->position();

    pos.deltaY(100);
    bras_->deplacer(pos, 50);
}

void VonFrey::placerFilament()
{
    Q_ASSERT(bras_ != 0);

    PositionBras pos(position());

    pos.setZ(0);
    bras_->deplacer(pos, 50);
}

void VonFrey::poserFilament()
{
    Q_ASSERT(bras_ != 0);

    PositionBras pos(position());

    pos.setZ(35);
    bras_->deplacer(pos, 50);
}

void VonFrey::releverFilament()
{
    Q_ASSERT(bras_ != 0);

    PositionBras pos = bras_->position();

    pos.setZ(0);
    bras_->deplacer(pos, 50);
}
